//
// Created by fl on 23-9-5.
//

#ifndef SIGMA_GUIDANCE_BODYTRACED_GUIDANCE_H
#define SIGMA_GUIDANCE_BODYTRACED_GUIDANCE_H

#include "controller_interface.h"
#include "matrix/math.hpp"
#include "mathlib/math/Limits.hpp"
#include "get_time.h"
#include "geo/geo.h"

class BodyTracedGuidance : public ControllerInterface {
public:
    BodyTracedGuidance() = default;
    ~BodyTracedGuidance() = default;

    // 设置体视线角信息，单位弧度
    bool set_LOS_status(double vertical, double horizontal, double ver_rate, double hor_rate) override;

    // 设置空速信息，单位m/s
    bool set_airspeed(double aspd) override;

    // 设置姿态角信息
    virtual bool set_att_status(float roll, float pitch, float yaw);

    // 获取控制指令
    mavlink_set_attitude_target_t get_att_target() const override;

    // 计算控制指令
    bool calc_att_thr_cmd(mavlink_set_attitude_target_t &cmd) override;

private:
    mavlink_set_attitude_target_t _att_target{};

    BlosInfo   _blos_info{};
    double     _airspeed{};

    float _pitch{};
    float _roll{};
    float _yaw{};

    bool  _tmg_vlos_enable{false};
    bool  _init_finish{true};

    float sdg_calc_roll();
    float sdg_calc_pitch_APG();
    float sdg_calc_pitch_IPG();
    float sdg_calc_throttle();


};


#endif //SIGMA_GUIDANCE_BODYTRACED_GUIDANCE_H
